!
! absfit - fitting absolute deviations
!
!
! Copyright © 2011 F.Hroch (hroch@physics.muni.cz)
!
! This file is part of Munipack.
!
! Munipack is free software: you can redistribute it and/or modify
! it under the terms of the GNU General Public License as published by
! the Free Software Foundation, either version 3 of the License, or
! (at your option) any later version.
! 
! Munipack is distributed in the hope that it will be useful,
! but WITHOUT ANY WARRANTY; without even the implied warranty of
! MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
! GNU General Public License for more details.
! 
! You should have received a copy of the GNU General Public License
! along with Munipack.  If not, see <http://www.gnu.org/licenses/>.
!


module absfit
  
  implicit none

  logical, parameter, private :: debug = .true.
  integer, parameter, private :: dbl = selected_real_kind(15)
  real(dbl), parameter, private :: rad = 57.295779513082322865_dbl
  real(dbl), dimension(:), allocatable, private :: x,y,u,v
  integer, private :: n

  private :: medfun,escale,erot

contains


  function medfun(p)

    real(dbl), dimension(:), intent(in) :: p
    real(dbl) :: medfun
    real(dbl) :: sres,s,c,rx,ry
    integer :: i

    c = p(3)*cos(p(4)/rad)
    s = p(3)*sin(p(4)/rad)

    sres = 0.0_dbl
    do i = 1, n
       rx = u(i) - (p(1) + c*x(i) - s*y(i))
       ry = v(i) - (p(2) + s*x(i) + c*y(i))
       sres = sres + abs(rx) + abs(ry)
!       write(*,'(9f10.5)') u(i),v(i),x(i),y(i),rx,ry,sres
    end do
    medfun = sres

  end function medfun


  subroutine  absmin(type,a,d,xc,yc,w,h,xx,yy,acen,dcen,sc,pa,rms,s0)

    use NelderMead
    use astrotrafo

    character(len=*),intent(in) :: type
    real(dbl),intent(in) :: xc,yc,w,h
    real(dbl),dimension(:),intent(in) :: a,d,xx,yy
    real(dbl),intent(inout) :: acen,dcen,sc,pa
    real(dbl),intent(out) :: rms,s0

    real(dbl) :: sc0,reqmin,tol
    integer :: iter,icount, numres, ifault
    type(TypeProjection) :: t
    integer, parameter :: npar = 4
    real(dbl),dimension(npar) :: p,p0,dp ! = (x0, y0, sc, pa)
    real(dbl), dimension(1) :: a0,d0

    reqmin = 100.0*epsilon(reqmin)

    ! Important: x,y,u,v arrays are a common working place for both absmin and medfun !!

    ! normalization
    sc0 = (w + h)/2.0_dbl
    allocate(x(size(xx)),y(size(yy)))
    x = (xx - xc)/sc0
    y = (yy - yc)/sc0

    ! init
    t%type = type
    t%acen = acen
    t%dcen = dcen
    t%xcen = 0.0_dbl
    t%ycen = 0.0_dbl
    t%scale = 1.0_dbl
    t%pa = 0.0_dbl

    ! initial estimation of pa
    p = (/0.0_dbl, 0.0_dbl, 1.0_dbl, 0.0_dbl/)

    n = size(a)
    allocate(u(n),v(n))

    call trafo(t,a,d,u,v)
!    call escale(u,v,x,y,p(3),ds)
!    call erot(u,v,x,y,p(4),da)

    do iter = 1,10

       ! estimate transformation by minimizing of absolute deviations
       p0 = p
       dp = 0.01*(p0 + 1.0)
       call nelmin(medfun,npar,p0,p,s0,reqmin,dp,1,1000,icount,&
            numres,ifault)

       sc = sc0/p(3)
       pa = p(4)
       rms = s0/n/1.41

       if( debug ) write(*,*) '# ifault=',ifault,s0
       if( debug ) write(*,*) '# p=',p

       ! correction of projection center
       call invtrafo(t,(/p(1)/),(/p(2)/),a0,d0)
       acen = a0(1)
       dcen = d0(1)

       write(*,'(a,en12.3,en13.3,f8.1,2f14.5)') "=AFIT> ",s0,sc,pa,acen,dcen

       tol = rms/20.0
       if( iter > 1 .and. abs(p(1)) < tol .and. abs(p(2)) < tol ) exit

       ! new projection by using of fitted parameters
       t%acen = acen
       t%dcen = dcen
       call trafo(t,a,d,u,v)

    end do

    deallocate(x,y,u,v)


  end subroutine absmin


  subroutine escale(x,y,u,v,c,dc)

    use robustmean

    implicit none
    real(dbl), dimension(:),intent(in) :: x,y,u,v
    real(dbl), intent(out) :: c,dc

    integer :: i,j,n
    real(dbl),dimension(:),allocatable :: cc
    real(dbl) :: d1,d2

    allocate(cc(size(x)**2))

    n = 0
    do i = 2, size(x)
       do j = 1,i-1
          d1 = sqrt((x(j) - x(i))**2 + (y(j) - y(i))**2)
          d2 = sqrt((u(j) - u(i))**2 + (v(j) - v(i))**2)
          n = n + 1
          cc(n) = d2/d1
!         if(debug) write(*,'(a,2es15.8,3f15.5)') '#',x(i),y(i),u(i),v(i),d2/d1
       end do
    enddo

    if( n > 1 ) then
       call rmean(cc(1:n),c,dc)
       dc = dc/sqrt(n - 1.0)
    else 
       c = 0.0
       dc = 0.0
    end if

    deallocate(cc)

    if( debug ) write(*,*) '# scale: c,dc=',c,dc

  end subroutine escale

  subroutine erot(x,y,u,v,f,df)

    use robustmean

    implicit none
    real(dbl), dimension(:),intent(in) :: x,y,u,v
    real(dbl), intent(out) :: f,df

    integer :: i,j,n
    real(dbl),dimension(:),allocatable :: cc
    real(dbl) :: d1,d2,x1,y1,x2,y2

    allocate(cc(size(x)**2))

    n = 0
    do i = 2, size(x)
       do j = 1,i-1
          x1 = x(j) - x(i)
          y1 = y(j) - y(i)
          d1 = sqrt(x1**2 + y1**2)
          x2 = u(j) - u(i)
          y2 = v(j) - v(i)
          d2 = sqrt(x2**2 + y2**2)
          n = n + 1
          cc(n) = (x1*x2 + y1*y2)/(d1*d2)
!          if(debug) write(*,'(a,2es15.8,3f15.5)') '# erot ',x(i),y(i),u(i),v(i),cc(n)
       end do
    enddo

    if( n > 1 ) then
       call rmean(cc(1:n),f,df)
       f = rad*acos(f)
       df = rad*asin(df/sqrt(n - 1.0))
    else
       f = 0.0
       df = 0.0
    end if

    if( debug ) write(*,*) '# angle: f [deg] = ',f,df

    deallocate(cc)

  end subroutine erot

end module absfit

